﻿#ifndef CALIBRATION_H
#define CALIBRATION_H

#include <opencv2/opencv.hpp>
#include <qmath.h>
#include <QString>
#include "RobotArm/robot_type.h"
#include "rigid_transform.h"
#include "tf/LinearMath/Transform.h"

#pragma execution_character_set("utf-8")

class Calibration
{
public:
    Calibration();

    //拟合圆
    void circleLeastFit(const std::vector<cv::Point2d> points, cv::Point2d &center, double &radius);

    //获取目标中心点
    cv::Point2d getMarkCenter();
    
    //获取目标偏移
    inline double getDeltaX() { return deltaX;}
    inline double getDeltaY() { return deltaY;}

    //计算旋转矩阵
    cv::Mat Calibration::getAffineTransform64f(std::string name,
                                               int index,
                                               const std::vector<cv::Point2f> src,
                                               const std::vector<cv::Point2f> dst);

    //
    bool leastSquaresFitting(const std::vector<cv::Point2d>& points);

    //
    cv::Mat LocalAffineEstimate(const std::vector<cv::Point2d>& shape1,
                                const std::vector<cv::Point2d>& shape2,
                                bool fullAfine);

    //
    void calculateTransformMatrix(const std::vector<cv::Point2d>& points_camera,
                                  const std::vector<cv::Point2d>& points_robot);

    bool add_station_transform_matrix(QString station_id, cv::Mat wrap_mat);
    bool remove_station_transform_matrix(QString station_id, cv::Mat wrap_mat);
    std::map<QString, cv::Mat> get_station_transform_matrix();
    
    bool add_station_reference_angle(QString station_id, float angle);
    bool remove_station_reference_angle(QString station_id, float angle);
    std::map<QString, float> get_station_reference_angle();

    bool add_station_reference_pose(QString station_id, RobotPoseXYZ pose);
    std::map<QString, RobotPoseXYZ> get_station_reference_pose();

    bool add_station_type(QString station_id, QString type);
    std::map<QString, QString> get_station_type();
    QString get_station_type(QString station_id);

    bool add_station_exposure_time(QString station_id, int value);
    int get_station_exposure_time(QString station_id);

    //根据相机定位，矫正点位
    //qr_point 二维码识别的坐标
    //qr_angle 二维码识别的角度
    //reference_pose 标定时的参考基准点
    //target_pose 移动的目标点位
    //corrected_target_pose 矫正后的点位
    bool correct_station_pose(QString station_id,
                              cv::Point2d qr_point,
                              float qr_angle,
                              RobotPoseXYZ reference_pose,
                              RobotPoseXYZ target_pose,
                              RobotPoseXYZ& corrected_target_pose);

    //根据相机定位，矫正点位 ,基准点已知
    //qr_point 二维码识别的坐标
    //qr_angle 二维码识别的角度
    //target_pose 要校准的目标点位
    //corrected_target_pose 矫正后的点位
    bool correct_station_pose(QString station_id,
                              cv::Point2d qr_point,
                              float qr_angle,
                              RobotPoseXYZ target_pose,
                              RobotPoseXYZ& corrected_target_pose);

    bool get_base_pose(QString station_id,
                       cv::Point2d qr_point,
                       float qr_angle,
                       RobotPoseXYZ& reference_pose,
                       RobotPoseXYZ& base_pose);

    //用于简易标定
    bool refine_station_pose(RobotPoseXYZ origin_pose,
                             RobotPoseXYZ correct_origin_pose,
                             RobotPoseXYZ target_pose,
                             RobotPoseXYZ& corrected_target_pose);


private:
    double deltaX;
    double deltaY;

    cv::Point2d mark_center;

    //calibration outcome

    std::map<QString, cv::Mat> stations_transform_matrix_;
    std::map<QString, float> stations_reference_angle;
    std::map<QString, RobotPoseXYZ> stations_reference_pose;
    std::map<QString, QString> stations_type;
    std::map<QString, int> stations_exposure_time;

    double A,B,C,D,E,F;

};

#endif // CALIBRATION_H
